#include <list>
#include <math.h>
#include "Octree.h"
#include "Triangle.h"
#include "ColoredPoint.h"

#pragma region Initialize Static Members
GLuint OctreeNode::nextCallList = 1;
#pragma endregion

Octree::Octree(list<Triangle*> l, float fovy, float aspect, int MaxThreshold, int MaxDepth) : fovy(fovy), aspect(aspect)
{
	for(list<Triangle*>::iterator iter = l.begin(); iter != l.end(); ++iter)
		shapes.push_back(*iter);
	float minx, miny, minz, maxx, maxy, maxz;
	FindMinAndMax(minx, miny, minz, maxx, maxy, maxz, &shapes);
	Head = new OctreeNode(minx, miny, minz, maxx, maxy, maxz, 1, MaxDepth, MaxThreshold, fovy, aspect, &shapes);
}

Octree::~Octree()
{
	if(Head)
		delete Head;
}

void Octree::ReturnVisibleShapesCallList(list<GLuint>* l, float camx, float camy, float camz, float frontx, float fronty, float frontz, float upx, float upy, float upz, float rightx, float righty, float rightz)
{
	l->clear();
	/*list<Triangle*> s = Head->GetShapes();
	int size = s.size();
	for(list<Triangle*>::iterator iter = s.begin(); iter != s.end(); ++iter)
		l->push_back(*iter);*/
	float height = tan(fovy/2);
	float width = aspect*height;
	vec<float, 3> topp1; topp1 = camx, camy, camz;
	vec<float, 3> topp2; topp2 = camx + frontx + (upx*height) + (rightx*width), camy + fronty + (upy*height) + (righty*width), camz + frontz + (upz*height) + (rightz*width);
	vec<float, 3> topp3; topp3 = camx + frontx + (upx*height) - (rightx*width), camy + fronty + (upy*height) - (righty*width), camz + frontz + (upz*height) - (rightz*width);
	vec<float, 3> bottomp1; bottomp1 = camx, camy, camz;
	vec<float, 3> bottomp2; bottomp2 = camx + frontx - (upx*height) + (rightx*width), camy + fronty - (upy*height) + (righty*width), camz + frontz - (upz*height) + (rightz*width);
	vec<float, 3> bottomp3; bottomp3 = camx + frontx - (upx*height) - (rightx*width), camy + fronty - (upy*height) - (righty*width), camz + frontz - (upz*height) - (rightz*width);
	vec<float, 3> rightp1; rightp1 = camx, camy, camz;
	vec<float, 3> rightp2; rightp2 = camx + frontx + (upx*height) + (rightx*width), camy + fronty + (upy*height) + (righty*width), camz + frontz + (upz*height) + (rightz*width);
	vec<float, 3> rightp3; rightp3 = camx + frontx - (upx*height) + (rightx*width), camy + fronty - (upy*height) + (righty*width), camz + frontz - (upz*height) + (rightz*width);
	vec<float, 3> leftp1; leftp1 = camx, camy, camz;
	vec<float, 3> leftp2; leftp2 = camx + frontx + (upx*height) - (rightx*width), camy + fronty + (upy*height) - (righty*width), camz + frontz + (upz*height) - (rightz*width);
	vec<float, 3> leftp3; leftp3 = camx + frontx - (upx*height) - (rightx*width), camy + fronty - (upy*height) - (righty*width), camz + frontz - (upz*height) - (rightz*width);

	vec<float, 3> topvec1; topvec1 = topp2 - topp1;
	vec<float, 3> topvec2; topvec2 = topp3 - topp1;
	vec<float, 3> bottomvec1; bottomvec1 = bottomp3 - bottomp1;
	vec<float, 3> bottomvec2; bottomvec2 = bottomp2 - bottomp1;
	vec<float, 3> rightvec1; rightvec1 = rightp3 - rightp1;
	vec<float, 3> rightvec2; rightvec2 = rightp2 - rightp1;
	vec<float, 3> leftvec1; leftvec1 = leftp2 - leftp1;
	vec<float, 3> leftvec2; leftvec2 = leftp3 - leftp1;

	vec<float, 3> topnorm; topnorm = topvec1.cross(topvec2);
	vec<float, 3> bottomnorm; bottomnorm = bottomvec1.cross(bottomvec2);
	vec<float, 3> rightnorm; rightnorm = rightvec1.cross(rightvec2);
	vec<float, 3> leftnorm; leftnorm = leftvec1.cross(leftvec2);
	Head->ReturnVisibleShapesCallList(l, camx, camy, camz, topnorm, bottomnorm, rightnorm, leftnorm);
}
void Octree::ReturnVisibleShapesFrustrum(list<Triangle*>* l, float camx, float camy, float camz, float frontx, float fronty, float frontz, float upx, float upy, float upz, float rightx, float righty, float rightz)
{
	l->clear();
	/*list<Triangle*> s = Head->GetShapes();
	int size = s.size();
	for(list<Triangle*>::iterator iter = s.begin(); iter != s.end(); ++iter)
		l->push_back(*iter);*/
	float height = tan(fovy/2);
	float width = aspect*height;
	vec<float, 3> topp1; topp1 = camx, camy, camz;
	vec<float, 3> topp2; topp2 = camx + frontx + (upx*height) + (rightx*width), camy + fronty + (upy*height) + (righty*width), camz + frontz + (upz*height) + (rightz*width);
	vec<float, 3> topp3; topp3 = camx + frontx + (upx*height) - (rightx*width), camy + fronty + (upy*height) - (righty*width), camz + frontz + (upz*height) - (rightz*width);
	vec<float, 3> bottomp1; bottomp1 = camx, camy, camz;
	vec<float, 3> bottomp2; bottomp2 = camx + frontx - (upx*height) + (rightx*width), camy + fronty - (upy*height) + (righty*width), camz + frontz - (upz*height) + (rightz*width);
	vec<float, 3> bottomp3; bottomp3 = camx + frontx - (upx*height) - (rightx*width), camy + fronty - (upy*height) - (righty*width), camz + frontz - (upz*height) - (rightz*width);
	vec<float, 3> rightp1; rightp1 = camx, camy, camz;
	vec<float, 3> rightp2; rightp2 = camx + frontx + (upx*height) + (rightx*width), camy + fronty + (upy*height) + (righty*width), camz + frontz + (upz*height) + (rightz*width);
	vec<float, 3> rightp3; rightp3 = camx + frontx - (upx*height) + (rightx*width), camy + fronty - (upy*height) + (righty*width), camz + frontz - (upz*height) + (rightz*width);
	vec<float, 3> leftp1; leftp1 = camx, camy, camz;
	vec<float, 3> leftp2; leftp2 = camx + frontx + (upx*height) - (rightx*width), camy + fronty + (upy*height) - (righty*width), camz + frontz + (upz*height) - (rightz*width);
	vec<float, 3> leftp3; leftp3 = camx + frontx - (upx*height) - (rightx*width), camy + fronty - (upy*height) - (righty*width), camz + frontz - (upz*height) - (rightz*width);

	vec<float, 3> topvec1; topvec1 = topp2 - topp1;
	vec<float, 3> topvec2; topvec2 = topp3 - topp1;
	vec<float, 3> bottomvec1; bottomvec1 = bottomp3 - bottomp1;
	vec<float, 3> bottomvec2; bottomvec2 = bottomp2 - bottomp1;
	vec<float, 3> rightvec1; rightvec1 = rightp3 - rightp1;
	vec<float, 3> rightvec2; rightvec2 = rightp2 - rightp1;
	vec<float, 3> leftvec1; leftvec1 = leftp2 - leftp1;
	vec<float, 3> leftvec2; leftvec2 = leftp3 - leftp1;

	vec<float, 3> topnorm; topnorm = topvec1.cross(topvec2);
	vec<float, 3> bottomnorm; bottomnorm = bottomvec1.cross(bottomvec2);
	vec<float, 3> rightnorm; rightnorm = rightvec1.cross(rightvec2);
	vec<float, 3> leftnorm; leftnorm = leftvec1.cross(leftvec2);
	Head->ReturnVisibleShapesFrustrum(l, camx, camy, camz, topnorm, bottomnorm, rightnorm, leftnorm);
}

void Octree::ReturnVisibleShapesSphere(list<Triangle*>* l, float camx, float camy, float camz, float radius)
{
	l->clear();
	Head->ReturnVisibleShapesSphere(l, camx, camy, camz, radius);
}

void Octree::FindMinAndMax(float& minx, float& miny, float& minz, float& maxx, float& maxy, float& maxz, list<Triangle*>* l)
{
	bool foundMinX = false;
	bool foundMinY = false;
	bool foundMinZ = false;
	bool foundMaxX = false;
	bool foundMaxY = false;
	bool foundMaxZ = false;

	int size = (int)l->size();

	for(list<Triangle*>::iterator iter = l->begin(); iter != l->end(); ++iter)
	{
		Triangle* t = (*iter);
		ColoredPoint p1 = t->GetPoint1();
		ColoredPoint p2 = t->GetPoint2();
		ColoredPoint p3 = t->GetPoint3();
		if(foundMinX)
		{
			if(p1.position[0] < minx)
			{
				minx = p1.position[0];
			}
			if(p2.position[0] < minx)
			{
				minx = p2.position[0];
			}
			if(p3.position[0] < minx)
			{
				minx = p3.position[0];
			}
		}
		else
		{
			foundMinX = true;
			if(p1.position[0] <= p2.position[0] && p1.position[0] <= p3.position[0])
			{
				minx = p1.position[0];
			}
			else if(p2.position[0] <= p1.position[0] && p2.position[0] <= p3.position[0])
			{
				minx = p2.position[0];
			}
			else if(p3.position[0] <= p1.position[0] && p3.position[0] <= p2.position[0])
			{
				minx = p3.position[0];
			}
		}
		if(foundMaxX)
		{
			if(p1.position[0] > maxx)
			{
				maxx = p1.position[0];
			}
			if(p2.position[0] > maxx)
			{
				maxx = p2.position[0];
			}
			if(p3.position[0] > maxx)
			{
				maxx = p3.position[0];
			}
		}
		else
		{
			foundMaxX = true;
			if(p1.position[0] >= p2.position[0] && p1.position[0] >= p3.position[0])
			{
				maxx = p1.position[0];
			}
			else if(p2.position[0] >= p1.position[0] && p2.position[0] >= p3.position[0])
			{
				maxx = p2.position[0];
			}
			else if(p3.position[0] >= p1.position[0] && p3.position[0] >= p2.position[0])
			{
				maxx = p3.position[0];
			}
		}
		if(foundMinY)
		{
			if(p1.position[1] < miny)
			{
				miny = p1.position[1];
			}
			if(p2.position[1] < miny)
			{
				miny = p2.position[1];
			}
			if(p3.position[1] < miny)
			{
				miny = p3.position[1];
			}
		}
		else
		{
			foundMinY = true;
			if(p1.position[1] <= p2.position[1] && p1.position[1] <= p3.position[1])
			{
				miny = p1.position[1];
			}
			else if(p2.position[1] <= p1.position[1] && p2.position[1] <= p3.position[1])
			{
				miny = p2.position[1];
			}
			else if(p3.position[1] <= p1.position[1] && p3.position[1] <= p2.position[1])
			{
				miny = p3.position[1];
			}
		}
		if(foundMaxY)
		{
			if(p1.position[1] > maxy)
			{
				maxy = p1.position[1];
			}
			if(p2.position[1] > maxy)
			{
				maxy = p2.position[1];
			}
			if(p3.position[1] > maxy)
			{
				maxy = p3.position[1];
			}
		}
		else
		{
			foundMaxY = true;
			if(p1.position[1] >= p2.position[1] && p1.position[1] >= p3.position[1])
			{
				maxy = p1.position[1];
			}
			else if(p2.position[1] >= p1.position[1] && p2.position[1] >= p3.position[1])
			{
				maxy = p2.position[1];
			}
			else if(p3.position[1] >= p1.position[1] && p3.position[1] >= p2.position[1])
			{
				maxy = p3.position[1];
			}
		}
		if(foundMinZ)
		{
			if(p1.position[2] < minz)
			{
				minz = p1.position[2];
			}
			if(p2.position[2] < minz)
			{
				minz = p2.position[2];
			}
			if(p3.position[2] < minz)
			{
				minz = p3.position[2];
			}
		}
		else
		{
			foundMinZ = true;
			if(p1.position[2] <= p2.position[2] && p1.position[2] <= p3.position[2])
			{
				minz = p1.position[2];
			}
			else if(p2.position[2] <= p1.position[2] && p2.position[2] <= p3.position[2])
			{
				minz = p2.position[2];
			}
			else if(p3.position[2] <= p1.position[2] && p3.position[2] <= p2.position[2])
			{
				minz = p3.position[2];
			}
		}
		if(foundMaxZ)
		{
			if(p1.position[2] > maxz)
			{
				maxz = p1.position[2];
			}
			if(p2.position[2] > maxz)
			{
				maxz = p2.position[2];
			}
			if(p3.position[2] > maxz)
			{
				maxz = p3.position[2];
			}
		}
		else
		{
			foundMaxZ = true;
			if(p1.position[2] >= p2.position[2] && p1.position[2] >= p3.position[2])
			{
				maxz = p1.position[2];
			}
			else if(p2.position[2] >= p1.position[2] && p2.position[2] >= p3.position[2])
			{
				maxz = p2.position[2];
			}
			else if(p3.position[2] >= p1.position[2] && p3.position[2] >= p2.position[2])
			{
				maxz = p3.position[2];
			}
		}
	}
}

OctreeNode::OctreeNode(float Xmin, float Ymin, float Zmin, float Xmax, float Ymax, float Zmax, int MyDepth, int MyMaxDepth, int MyThreshold, float fovy, float aspect, list<Triangle*>* l) : fovy(fovy), aspect(aspect)
{
    callList = NULL;
	for(int i = 0; i < 8; i++)
		Children[i] = 0;
	minX = Xmin;
	minY = Ymin;
	minZ = Zmin;
	maxX = Xmax;
	maxY = Ymax;
	maxZ = Zmax;
	Depth = MyDepth;
	MaxDepth = MyMaxDepth;
	Threshold = MyThreshold;

	int size = (int) l->size();

	for(int i = 0; i < size; i++)
	{
		Triangle* t = l->front();
		l->pop_front();
		ColoredPoint p1 = t->GetPoint1();
		ColoredPoint p2 = t->GetPoint2();
		ColoredPoint p3 = t->GetPoint3();
		if(p1.position[0] >= minX && p2.position[0] >= minX && p3.position[0] >= minX)
		{
			if(p1.position[0] <= maxX && p2.position[0] <= maxX && p3.position[0] <= maxX)
			{
				if(p1.position[1] >= minY && p2.position[1] >= minY && p3.position[1] >= minY)
				{
					if(p1.position[1] <= maxY && p2.position[1] <= maxY && p3.position[1] <= maxY)
					{
						if(p1.position[2] >= minZ && p2.position[2] >= minZ && p3.position[2] >= minZ)
						{
							if(p1.position[2] <= maxZ && p2.position[2] <= maxZ && p3.position[2] <= maxZ)
							{
								shapes.push_back(t);
							}
							else
							{
								l->push_back(t);
							}
						}
						else
						{
							l->push_back(t);
						}
					}
					else
					{
						l->push_back(t);
					}
				}
				else
				{
					l->push_back(t);
				}
			}
			else
			{
				l->push_back(t);
			}
		}
		else
		{
			l->push_back(t);
		}
	}

	if((MaxDepth < 0 || Depth < MaxDepth) && (int) shapes.size() > Threshold)
	{
		int child = 0;
		float dx = (maxX - minX)/2;
		float dy = (maxY - minY)/2;
		float dz = (maxZ - minZ)/2;
		for(int i = 0; i < 2; i++)
		{
			for(int j = 0; j < 2; j++)
			{
				for(int k = 0; k < 2; k++)
				{
					Children[child++] = new OctreeNode(minX + (dx * i), minY + (dy * j), minZ + (dz * k), minX + (dx * (i + 1)), minY + (dy * (j + 1)), minZ + (dz * (k + 1)), Depth+1, MaxDepth, Threshold, fovy, aspect, &shapes);
				}
			}
		}
        callList = OctreeNode::GetNextCallList();
        glNewList(callList, GL_COMPILE);
        for (list<Triangle*>::iterator iter = shapes.begin(); 
             iter != shapes.end(); ++iter)
        {
            (*iter)->Draw();
            // delete *iter;
            // iter = shapes.erase(iter);
        }
        glEndList();
	}
}

OctreeNode::~OctreeNode()
{
	for(int i = 0; i < 8; i++)
		if(Children[i])
			delete Children[i];
    if (callList != NULL)
        glDeleteLists(callList, 1);
}

void OctreeNode::SetMinX(float Xmin)
{
	minX = Xmin;
}

void OctreeNode::SetMinY(float Ymin)
{
	minY = Ymin;
}

void OctreeNode::SetMinZ(float Zmin)
{
	minZ = Zmin;
}

void OctreeNode::SetMaxX(float Xmax)
{
	maxX = Xmax;
}

void OctreeNode::SetMaxY(float Ymax)
{
	maxY = Ymax;
}

void OctreeNode::SetMaxZ(float Zmax)
{
	maxZ = Zmax;
}

float OctreeNode::GetMinX()
{
	return minX;
}

float OctreeNode::GetMinY()
{
	return minY;
}

float OctreeNode::GetMinZ()
{
	return minZ;
}

float OctreeNode::GetMaxX()
{
	return maxX;
}

float OctreeNode::GetMaxY()
{
	return maxY;
}

float OctreeNode::GetMaxZ()
{
	return maxZ;
}

list<Triangle*> OctreeNode::GetShapes()
{
	return shapes;
}

bool OctreeNode::IsInside(float x, float y, float z)
{
	if(x >= minX && x <= maxX && y >= minY && y <=maxY && z >= minZ && z <= maxZ)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void OctreeNode::ReturnVisibleShapesSphere(list<Triangle*>* l, float camx, float camy, float camz, float radius)
{
	if(IsInside(camx, camy, camz))
	{
		for(list<Triangle*>::iterator iter = shapes.begin(); iter != shapes.end(); ++iter)
		{
			l->push_back(*iter);
		}
		for(int i = 0; i < 8; i++)
		{
			if(Children[i])
				Children[i]->ReturnVisibleShapesSphere(l, camx, camy, camz, radius);
		}
	}
	else if(sqrt(powf((minX + ((maxX - minX) / 2)) - camx, 2.0) + powf((minY + ((maxY - minY) / 2)) - camy, 2.0) + powf((minZ + ((maxZ - minZ) / 2)) - camz, 2.0)) <= radius)
	{
		for(list<Triangle*>::iterator iter = shapes.begin(); iter != shapes.end(); ++iter)
		{
			l->push_back(*iter);
		}
		for(int i = 0; i < 8; i++)
		{
			if(Children[i])
				Children[i]->ReturnVisibleShapesSphere(l, camx, camy, camz, radius);
		}
	}
}

void OctreeNode::ReturnVisibleShapesFrustrum(list<Triangle*>* l, float camx, float camy, float camz, vec<float, 3> topnorm, vec<float,3> bottomnorm, vec<float, 3> rightnorm, vec<float, 3> leftnorm)
{
	float dx = maxX - minX;
	float dy = maxY - minY;
	float dz = maxZ - minZ;
	vec<float, 3> norms[4] = {topnorm, bottomnorm, rightnorm, leftnorm};

	vec<float, 3> cam; cam = camx, camy, camz;
	bool exclude[4] = {true, true, true, true};
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 2; j++)
		{
			for(int k = 0; k < 2; k++)
			{
				for(int l = 0; l < 2; l++)
				{
					vec<float, 3> pos; pos = minX+(dx*j), minY+(dy*k), minZ+(dz*l);
					if(norms[i].inner(pos) - norms[i].inner(cam) <= 0)
						exclude[i] = false;
				}
			}
		}
	}

	if(!exclude[0] && !exclude[1] && !exclude[2] && !exclude[3])
	{
		for(list<Triangle*>::iterator iter = shapes.begin(); iter != shapes.end(); ++iter)
		{
			l->push_back(*iter);
		}
		for(int i = 0; i < 8; i++)
		{
			if(Children[i])
				Children[i]->ReturnVisibleShapesFrustrum(l, camx, camy, camz, topnorm, bottomnorm, rightnorm, leftnorm);
		}
	}
}
void OctreeNode::ReturnVisibleShapesCallList(list<GLuint>* l, float camx, float camy, float camz, vec<float, 3> topnorm, vec<float,3> bottomnorm, vec<float, 3> rightnorm, vec<float, 3> leftnorm)
{
	float dx = maxX - minX;
	float dy = maxY - minY;
	float dz = maxZ - minZ;
	vec<float, 3> norms[4] = {topnorm, bottomnorm, rightnorm, leftnorm};

	vec<float, 3> cam; cam = camx, camy, camz;
	bool exclude[4] = {true, true, true, true};
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 2; j++)
		{
			for(int k = 0; k < 2; k++)
			{
				for(int l = 0; l < 2; l++)
				{
					vec<float, 3> pos; pos = minX+(dx*j), minY+(dy*k), minZ+(dz*l);
					if(norms[i].inner(pos) - norms[i].inner(cam) <= 0)
						exclude[i] = false;
				}
			}
		}
	}

	if(!exclude[0] && !exclude[1] && !exclude[2] && !exclude[3])
	{
        l->push_back(callList);
        /* comment if using call list recursion */
		for(int i = 0; i < 8; i++)
		{
			if(Children[i])
				Children[i]->ReturnVisibleShapesCallList(l, camx, camy, camz, topnorm, bottomnorm, rightnorm, leftnorm);
		}
        /* comment if using call list recursion */
	}
}
